//
// Copyright (c) Microsoft Corporation.  All rights reserved.
//
//
// Use of this source code is subject to the terms of the Microsoft shared
// source or premium shared source license agreement under which you licensed
// this source code. If you did not accept the terms of the license agreement,
// you are not authorized to use this source code. For the terms of the license,
// please see the license agreement between you and Microsoft or, if applicable,
// see the SOURCE.RTF on your install media or the root of your tools installation.
// THE SOURCE CODE IS PROVIDED "AS IS", WITH NO WARRANTIES OR INDEMNITIES.
//
#include <windows.h>
#include <commdlg.h>
#include <streams.h>
#include <initguid.h>
#include "motionfilter.h"

// {4ED94EC4-45FF-49d4-A924-E78F22532505}
DEFINE_GUID(CLSID_MotionFilter, 
0x4ed94ec4, 0x45ff, 0x49d4, 0xa9, 0x24, 0xe7, 0x8f, 0x22, 0x53, 0x25, 0x5);


// Setup data
const AMOVIESETUP_MEDIATYPE sudPinTypes[] =
{
    {&MEDIATYPE_Video,&MEDIASUBTYPE_RGB565},
    {&MEDIATYPE_Video,&MEDIASUBTYPE_YUY2},
};

const AMOVIESETUP_PIN sudPins[] =
{
    {
        L"VideoInput",                     // Pin string name
        FALSE,                      // Is it rendered
        FALSE,                      // Is it an output
        FALSE,                      // Allowed none
        FALSE,                      // Likewise many
        &CLSID_NULL,                // Connects to filter
        NULL,                        // Connects to pin
        2,                          // Number of types
        sudPinTypes                // Pin information
    },
    {
        L"VideoOutput",                     // Pin string name
        FALSE,                      // Is it rendered
        FALSE,                      // Is it an output
        FALSE,                      // Allowed none
        FALSE,                      // Likewise many
        &CLSID_NULL,                // Connects to filter
        NULL,                        // Connects to pin
        2,                          // Number of types
        sudPinTypes                // Pin information
    }
};

const AMOVIESETUP_FILTER sudMotion =
{
    &CLSID_MotionFilter,     // Filter CLSID
    L"Motion Filter",        // String name
    MERIT_DO_NOT_USE,           // Filter merit
    2,                          // Number pins
    sudPins                    // Pin details
};

CFactoryTemplate g_Templates[]= {
    L"MotionFilter", &CLSID_MotionFilter, CMotionFilter::CreateInstance, NULL, &sudMotion
};
int g_cTemplates = 1;

CMotionFilter::CMotionFilter( IUnknown * pOuter, HRESULT * phr, BOOL ModifiesData ) :
    CTransInPlaceFilter( TEXT("MotionFilter"), (IUnknown*) pOuter, CLSID_MotionFilter, phr)
{
    m_pPreviousSample = NULL;
    m_Stride = 0;
    m_Width = 0;
    m_Height = 0;
}

CUnknown * WINAPI CMotionFilter::CreateInstance(LPUNKNOWN punk, HRESULT *phr)
{
    HRESULT hr;
    if (!phr)
        phr = &hr;
    
    CMotionFilter *pNewObject = new CMotionFilter(punk, phr, FALSE);
    if (pNewObject == NULL)
        *phr = E_OUTOFMEMORY;

    return pNewObject;
}

HRESULT CMotionFilter::CheckInputType(const CMediaType *pmt)
{
    if(pmt->majortype == MEDIATYPE_Video && 
        (pmt->subtype == MEDIASUBTYPE_RGB565 || pmt->subtype == MEDIASUBTYPE_YUY2) &&
        pmt->formattype == FORMAT_VideoInfo)
        return S_OK;
    else return E_FAIL;
}

HRESULT CMotionFilter::SetMediaType(PIN_DIRECTION direction, const CMediaType *pmt)
{
    HRESULT hr = S_OK;

    if(pmt->majortype == MEDIATYPE_Video && 
        (pmt->subtype == MEDIASUBTYPE_RGB565 || pmt->subtype == MEDIASUBTYPE_YUY2) &&
        pmt->formattype == FORMAT_VideoInfo &&
        pmt->cbFormat >= sizeof(VIDEOINFOHEADER) &&
        pmt->pbFormat != NULL)
    {
        m_mt = *pmt;

        VIDEOINFOHEADER *vih = (VIDEOINFOHEADER *) pmt->pbFormat;

        m_Width = vih->bmiHeader.biWidth;
        m_Height = vih->bmiHeader.biHeight;

        // since we're RGB565 or YUY2 and do not have the video renderer in the graph (so we can't be using DDraw)
        // the stride is the width * bytes per pixel
        m_Stride = m_Width *2;
    }
    else hr = E_FAIL;

    return hr;
}

float DiffRGB565Image(BYTE* pSrcBits, int srcStride, BYTE* pDstBits, int dstStride, int width, int height)
{
    if (!pSrcBits || !pDstBits)
        return -1;

    BYTE r1, g1, b1;
    BYTE r2, g2, b2;
    float peldiff = 0;
    BYTE* ptr;

    if(srcStride == 0)
        srcStride = width * 2;

    if(dstStride == 0)
        dstStride = width * 2;

    for(int i = 0; i < height; i++)
    {
        for(int j = 0; j < width; j++)
        {
            // Split r:5 g:6 b:5
            ptr = pSrcBits + i*srcStride + j*2;
            WORD rgb = *(WORD*)ptr;
            r1 = (rgb >> 8) & 0xF8;
            r1 |= r1 >> 5;
            g1 = (rgb >> 3) & 0xFC;
            g1 |= g1 >> 6;
            b1 = (rgb << 3) & 0xF8;
            b1 |= b1 >> 5;


            ptr = pDstBits + i*dstStride + j*2;
            rgb = *(WORD*)ptr;
            r2 = (rgb >> 8) & 0xF8;
            r2 |= r2 >> 5;
            g2 = (rgb >> 3) & 0xFC;
            g2 |= g2 >> 6;
            b2 = (rgb << 3) & 0xF8;
            b2 |= b2 >> 5;

#if DIFF_ABS
            peldiff += abs(r1 - r2) + abs(g1 - g2) + abs(b1 - b2);
#else
            peldiff += (float)sqrt((float)(r1 - r2)*(r1 - r2) + (float)(g1 - g2)*(g1 - g2) + (float)(b1 - b2)*(b1 - b2));
#endif
        }
    }
    // Calculate error per bit
    peldiff = peldiff/(width*height*16);
    return peldiff;
}

#define YUV2RGB(y, u, v, r, g, b) \
r = 1.164f*(y - 16) + 1.596f*(v - 128); \
g = 1.164f*(y - 16) - 0.813f*(v - 128) - 0.391f*(u - 128); \
b = 1.164f*(y - 16) + 2.018f*(u - 128)

#define CLAMP(x, lo, hi) \
    if (x < lo) \
        x = lo; \
    if (x > hi) \
        x = hi;

float DiffYUY2Image(BYTE* pSrcBits, int srcStride, BYTE* pDstBits, int dstStride, int width, int height)
{
    if (!pSrcBits || !pDstBits)
        return -1;

    float peldiff = 0;
    unsigned char *ptr;
    unsigned char* dstptr;
    float srcr1, srcg1, srcb1;
    float srcr2, srcg2, srcb2;
    float dstr1, dstg1, dstb1;
    float dstr2, dstg2, dstb2;

    float y0, u0, v0, y1;

    if(srcStride == 0)
        srcStride = width * 2;

    if(dstStride == 0)
        dstStride = width * 2;

    for(int i = 0; i < height; i++)
    {
        // we're dealing with two pixels per surface at a time, so we're
        // converting four pixels from YUV to RGB and calculating the diff.
        for(int j = 0; j < width; j += 2)
        {
            ptr = (pSrcBits + i*srcStride + j*2);
            dstptr = (pDstBits + i*dstStride + j*2);

            y0 = (float)*ptr;
            u0 = (float)*(ptr + 1);
            y1 = (float)*(ptr + 2);
            v0 = (float)*(ptr + 3);

            // Convert 1st and second pixel of the source YUV surface
            YUV2RGB(y0, u0, v0, srcr1, srcg1, srcb1);
            YUV2RGB(y1, u0, v0, srcr2, srcg2, srcb2);
    
            CLAMP(srcr1, 0, 255);
            CLAMP(srcg1, 0, 255);
            CLAMP(srcb1, 0, 255);
            CLAMP(srcr2, 0, 255);
            CLAMP(srcg2, 0, 255);
            CLAMP(srcb2, 0, 255);

            y0 = (float)*dstptr;
            u0 = (float)*(dstptr + 1);
            y1 = (float)*(dstptr + 2);
            v0 = (float)*(dstptr + 3);

            // Convert 1st and second pixel of the dest YUV surface
            YUV2RGB(y0, u0, v0, dstr1, dstg1, dstb1);
            YUV2RGB(y1, u0, v0, dstr2, dstg2, dstb2);
            CLAMP(dstr1, 0, 255);
            CLAMP(dstg1, 0, 255);
            CLAMP(dstb1, 0, 255);
            CLAMP(dstr2, 0, 255);
            CLAMP(dstg2, 0, 255);
            CLAMP(dstb2, 0, 255);

#if DIFF_ABS
            peldiff += abs(srcr1 - dstr1) + abs(srcg1 - dstg1) + abs(srcb1 - dstb1);
            peldiff += abs(srcr2 - dstr2) + abs(srcg2 - dstg2) + abs(srcb2 - dstb2);
#else
            peldiff += (float)sqrt((float)(srcr1 - dstr1)*(srcr1 - dstr1) + (float)(srcg1 - dstg1)*(srcg1 - dstg1) + (float)(srcb1 - dstb1)*(srcb1 - dstb1));
            peldiff += (float)sqrt((float)(srcr2 - dstr2)*(srcr2 - dstr2) + (float)(srcg2 - dstg2)*(srcg2 - dstg2) + (float)(srcb2 - dstb2)*(srcb2 - dstb2));
#endif

        }
    }
    // Calculate error per bit
    peldiff = peldiff/(width*height*24);
    return peldiff;
}

HRESULT CMotionFilter::Transform(IMediaSample *pMediaSample)
{
    HRESULT hr = S_OK;

    if(pMediaSample)
    {
        BYTE *pCurrentBits = NULL;
        hr = pMediaSample->GetPointer(&pCurrentBits);

        if(SUCCEEDED(hr) && m_pPreviousSample)
        {
            if(m_bPreviousSampleSet)
            {
                float diff = 0;

                if(m_mt.subtype == MEDIASUBTYPE_RGB565)
                    diff = DiffRGB565Image(m_pPreviousSample, m_Stride, pCurrentBits, m_Stride, m_Width, m_Height);
                else if(m_mt.subtype == MEDIASUBTYPE_YUY2)
                    diff = DiffYUY2Image(m_pPreviousSample, m_Stride, pCurrentBits, m_Stride, m_Width, m_Height);

                if(diff > THRESHOLD)
                    NotifyEvent(EC_MOTIONDETECTED, 0, 0);
            }
            else m_bPreviousSampleSet = TRUE;

            memcpy(m_pPreviousSample, pCurrentBits, m_Stride * m_Height);
        }
        else NotifyEvent(EC_ERRORABORT, 0, 0);    

    }
    return S_OK;
}

HRESULT CMotionFilter::Pause()
{
    if(m_pPreviousSample)
    {
        delete[] m_pPreviousSample;
        m_pPreviousSample = NULL;
    }

    m_pPreviousSample = new BYTE[m_Stride * m_Height];
    m_bPreviousSampleSet = FALSE;

    return CTransInPlaceFilter::Pause();
}

HRESULT CMotionFilter::Stop()
{
    if(m_pPreviousSample)
    {
        delete[] m_pPreviousSample;
        m_pPreviousSample = NULL;
    }

    m_bPreviousSampleSet = FALSE;

    return CTransInPlaceFilter::Stop();
}

STDAPI DllRegisterServer()
{
    return AMovieDllRegisterServer2( TRUE );
}

STDAPI DllUnregisterServer()
{
    return AMovieDllRegisterServer2( FALSE );
}

extern "C" BOOL WINAPI DllEntryPoint(HINSTANCE, ULONG, LPVOID);

BOOL APIENTRY DllMain(HANDLE hModule, 
                    DWORD  dwReason, 
                    LPVOID lpReserved)
{
    return DllEntryPoint((HINSTANCE)(hModule), dwReason, lpReserved);
}

